///////////////////////////////////////////////////////////////////////////////
//
// Lynxmotion SSC-32 Servo Controller Service
//
// Based on code from the Lynxmotion web site arm.1.2.zip
// Modified with the permission of Lynxmotion
//
// Updated Nov-2007 by Trevor Taylor, Software Technology
//
// This code is Open Source
//
// Contract Identifier and Namespace modified in Feb 2008 to avoid a conflict
// with the CoroWare ClassPack which also includes an SSC-32 service
//
///////////////////////////////////////////////////////////////////////////////

using System;
using System.Collections.Generic;
using System.IO.Ports;
using Microsoft.Dss.Core.Attributes;

namespace ProMRDS.Robotics.Lynxmotion.Ssc32
{
    
    [DataContract()]
    public class SSC32State
    {
        internal static int NUM_CHANNELS = 32;
        private int[] _servoPulseCache = new int[NUM_CHANNELS];

        public SSC32State()
        {
            // The SSC32 can not know initially the pulse code position
            // of the servos it controls, until they are initially set,
            // we will store those values as -1 in the internal cache
            for (int i = 0; i < _servoPulseCache.Length; i++)
                _servoPulseCache[i] = -1;
        }

        #region Servo Pulse Data Members
        [DataMember]
        public int ServoPulse0
        {
            get { return _servoPulseCache[0]; }
            set { SetServoPulseValue(0, value); }
        }

        [DataMember]
        public int ServoPulse1
        {
            get { return _servoPulseCache[1]; }
            set { SetServoPulseValue(1, value); }
        }

        [DataMember]
        public int ServoPulse2
        {
            get { return _servoPulseCache[2]; }
            set { SetServoPulseValue(2, value); }
        }

        [DataMember]
        public int ServoPulse3
        {
            get { return _servoPulseCache[3]; }
            set { SetServoPulseValue(3, value); }
        }

        [DataMember]
        public int ServoPulse4
        {
            get { return _servoPulseCache[4]; }
            set { SetServoPulseValue(4, value); }
        }

        [DataMember]
        public int ServoPulse5
        {
            get { return _servoPulseCache[5]; }
            set { SetServoPulseValue(5, value); }
        }

        [DataMember]
        public int ServoPulse6
        {
            get { return _servoPulseCache[6]; }
            set { SetServoPulseValue(6, value); }
        }

        [DataMember]
        public int ServoPulse7
        {
            get { return _servoPulseCache[7]; }
            set { SetServoPulseValue(7, value); }
        }

        [DataMember]
        public int ServoPulse8
        {
            get { return _servoPulseCache[8]; }
            set { SetServoPulseValue(8, value); }
        }

        [DataMember]
        public int ServoPulse9
        {
            get { return _servoPulseCache[9]; }
            set { SetServoPulseValue(9, value); }
        }

        [DataMember]
        public int ServoPulse10
        {
            get { return _servoPulseCache[10]; }
            set { SetServoPulseValue(10, value); }
        }

        [DataMember]
        public int ServoPulse11
        {
            get { return _servoPulseCache[11]; }
            set { SetServoPulseValue(11, value); }
        }

        [DataMember]
        public int ServoPulse12
        {
            get { return _servoPulseCache[12]; }
            set { SetServoPulseValue(12, value); }
        }

        [DataMember]
        public int ServoPulse13
        {
            get { return _servoPulseCache[13]; }
            set { SetServoPulseValue(13, value); }
        }

        [DataMember]
        public int ServoPulse14
        {
            get { return _servoPulseCache[14]; }
            set { SetServoPulseValue(14, value); }
        }

        [DataMember]
        public int ServoPulse15
        {
            get { return _servoPulseCache[15]; }
            set { SetServoPulseValue(15, value); }
        }

        [DataMember]
        public int ServoPulse16
        {
            get { return _servoPulseCache[16]; }
            set { SetServoPulseValue(16, value); }
        }

        [DataMember]
        public int ServoPulse17
        {
            get { return _servoPulseCache[17]; }
            set { SetServoPulseValue(17, value); }
        }

        [DataMember]
        public int ServoPulse18
        {
            get { return _servoPulseCache[18]; }
            set { SetServoPulseValue(18, value); }
        }

        [DataMember]
        public int ServoPulse19
        {
            get { return _servoPulseCache[19]; }
            set { SetServoPulseValue(19, value); }
        }

        [DataMember]
        public int ServoPulse20
        {
            get { return _servoPulseCache[20]; }
            set { SetServoPulseValue(20, value); }
        }

        [DataMember]
        public int ServoPulse21
        {
            get { return _servoPulseCache[21]; }
            set { SetServoPulseValue(21, value); }
        }

        [DataMember]
        public int ServoPulse22
        {
            get { return _servoPulseCache[22]; }
            set { SetServoPulseValue(22, value); }
        }

        [DataMember]
        public int ServoPulse23
        {
            get { return _servoPulseCache[23]; }
            set { SetServoPulseValue(23, value); }
        }

        [DataMember]
        public int ServoPulse24
        {
            get { return _servoPulseCache[24]; }
            set { SetServoPulseValue(24, value); }
        }

        [DataMember]
        public int ServoPulse25
        {
            get { return _servoPulseCache[25]; }
            set { SetServoPulseValue(25, value); }
        }

        [DataMember]
        public int ServoPulse26
        {
            get { return _servoPulseCache[26]; }
            set { SetServoPulseValue(26, value); }
        }

        [DataMember]
        public int ServoPulse27
        {
            get { return _servoPulseCache[27]; }
            set { SetServoPulseValue(27, value); }
        }

        [DataMember]
        public int ServoPulse28
        {
            get { return _servoPulseCache[28]; }
            set { SetServoPulseValue(28, value); }
        }

        [DataMember]
        public int ServoPulse29
        {
            get { return _servoPulseCache[29]; }
            set { SetServoPulseValue(29, value); }
        }

        [DataMember]
        public int ServoPulse30
        {
            get { return _servoPulseCache[30]; }
            set { SetServoPulseValue(30, value); }
        }

        [DataMember]
        public int ServoPulse31
        {
            get { return _servoPulseCache[31]; }
            set { SetServoPulseValue(31, value); }
        }

        internal void SetServoPulseValue(int index, int value)
        {
            if (index < 0 || index >= NUM_CHANNELS)
                throw new ArgumentException("Servo index out of bounds");
            
            // SSC32 pulse values must be in the range of 500 to 2500
            // We will internally correct values that go outside this range
            if (value < 500)
                value = 500;
            if (value > 2500)
                value = 2500;

            _servoPulseCache[index] = value;
        }
        #endregion

        internal int[] GetAllServoPulses()
        {
            return _servoPulseCache;
        }
    }

    [DataContract]
    public class SSC32Command
    {
        private SSC32Commands _commandType;

        [DataMember]
        public SSC32Commands CommandType
        {
            get { return _commandType; }
            // TT Jul-2008 - Change for July CTP of 2.0
            // Properties cannot be Read-Only
            set { }
        }

        public SSC32Command()
        {
        }

        public SSC32Command(SSC32Commands command)
        {
            _commandType = command;
        }
    }

    [DataContract]
    public class SSC32ResponseType : SSC32Command
    {
        public SSC32ResponseType() { }
        public SSC32ResponseType(SSC32Commands command)
            : base(command)
        {
        }
    }

    [DataContract]
    public class SSC32PulseWidthResponse : SSC32ResponseType
    {
        private int[] _pulseWidths;

        public SSC32PulseWidthResponse()
            : base(SSC32Commands.QueryPulseWidth)
        {
        }

        public SSC32PulseWidthResponse(int[] pulseWidths)
            : base(SSC32Commands.QueryPulseWidth)
        {
            this._pulseWidths = pulseWidths;
        }

        //500-2500 microseconds
        [DataMember]
        public int[] PulseWidths
        {
            get { return _pulseWidths; }
            set { _pulseWidths = value; }
        }
    }

    [DataContract]
    public class SSC32QueryPulseWidth : SSC32Command
    {   
        private int[] _channels;

        public SSC32QueryPulseWidth()
            : base(SSC32Commands.QueryPulseWidth)
        {
        }

        public SSC32QueryPulseWidth(int[] channels)
            : base(SSC32Commands.QueryPulseWidth)
        {
            this._channels = channels;
        }

        //Channels 0-31
        [DataMember]
        public int[] Channels
        {
            get { return _channels; }
            set { _channels = value; }
        }
    }

    [DataContract]
    public class SSC32ServoMove : SSC32Command
    {
        private int[] _channels;
        private int[] _pulseWidths;
        private int[] _speeds;
        private int _time;

        public SSC32ServoMove(int channel, int pulseWidth)
            : base(SSC32Commands.ServoMove)
        {
            this._channels = new int[] { channel };
            this._pulseWidths = new int[] { pulseWidth };
            this._speeds = null;
            this._time = -1;
        }

        //speed and time are optional arguments. Use -1 as a value if either is not needed.
        public SSC32ServoMove(int channel, int pulseWidth, int speed, int time)
            : base(SSC32Commands.ServoMove)
        {
            this._channels = new int[] { channel };
            this._pulseWidths = new int[] { pulseWidth };
            this._speeds = new int[] { speed };
            this._time = time;
        }

        //channels, pulsewidths and speeds must be of the same length.
        public SSC32ServoMove(int[] channels, int[] pulseWidths)
            : base(SSC32Commands.ServoMove)
        {
            this._channels = channels;
            this._pulseWidths = pulseWidths;
            this._speeds = null;
            this._time = -1;
        }

        //channels, pulsewidths and speeds must be of the same length.
        //Use -1 as a value of a speed or time if it is not needed.
        public SSC32ServoMove(int[] channels, int[] pulseWidths, int[] speeds, int time)
            : base(SSC32Commands.ServoMove)
        {
            this._channels = channels;
            this._pulseWidths = pulseWidths;
            this._speeds = speeds;
            this._time = time;
        }

        public SSC32ServoMove()
            : base(SSC32Commands.ServoMove)
        {
        }

        //Channels 0-31
        [DataMember]
        public int[] Channels
        {
            get { return _channels; }
            set { _channels = value; }
        }

        //500-2500 microseconds
        [DataMember]
        public int[] PulseWidths
        {
            get { return _pulseWidths; }
            set { _pulseWidths = value; }
        }

        //uS per second
        [DataMember]
        public int[] Speeds
        {
            get { return _speeds; }
            set { _speeds = value; }
        }

        //in mS, 65535 max
        [DataMember]
        public int Time
        {
            get { return _time; }
            set { _time = value; }
        }
    }

    #region Serial Port Data Structures
    [DataContract]
    public class SerialPortConfig
    {
        //Serial port configuration parameters

        //Baud rate is 115.2k with both jumpers
        private int _baudRate = 115200;
        private Parity _parity = Parity.None;
        private int _dataBits = 8;
        private StopBits _stopBits = StopBits.One;
        // IMPORTANT: This is the COM port that will be used by the arm.
        // It is hard-coded here. There should be a config file associated
        // with this service, but this has not been implemented yet.
        private string _portName = "COM1";
        private int _timeout = 1000;

        /// <summary>
        /// Serial Communications Port 
        /// </summary>
        [DataMember]
        public int CommPort
        {
            get
            {
                try
                {
                    return Int32.Parse(_portName.Substring(_portName.Length - 1));
                }
                catch (Exception)
                {
                    // If we can't distinguish the port from the name, go negative
                    return -1;
                }
            }
            set { _portName = "COM" + value.ToString(); }
        }

        [DataMember]
        public string PortName
        {
            get { return _portName; }
            set { _portName = value; }
        }

        /// <summary>
        /// Baud Rate
        /// </summary>
        [DataMember]
        public int BaudRate
        {
            get { return _baudRate; }
            set { _baudRate = value; }
        }

        /// <summary>
        /// Specifies the Parity bit for a Serial Port.
        /// </summary>
        [DataMember]
        public Parity Parity
        {
            get { return _parity; }
            set { _parity = value; }
        }

        [DataMember]
        public int DataBits
        {
            get { return _dataBits; }
            set { _dataBits = value; }
        }

        [DataMember]
        public StopBits StopBits
        {
            get { return _stopBits; }
            set { _stopBits = value; }
        }

        [DataMember]
        public int QueryTimeOut
        {
            get { return _timeout; }
            set { _timeout = value; }
        }
    }
    #endregion
}
